package mmesh;

import java.io.BufferedOutputStream;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;

import processing.core.PGraphics;
import processing.core.PMatrix3D;
import processing.core.PVector;
import hdgeom.primitives.AbstractShape;
import hdgeom.primitives.Box;
import hdgeom.primitives.Plane;
import hdgeom.primitives.TrianglePreCalc;

public class MMesh extends AbstractShape {

	public float[] xc;
	public float[] yc;
	public float[] zc;
	public float[] xn;
	public float[] yn;
	public float[] zn;
	public int[] nodeNbs;
	public int[] triangles = new int[0];
	public int[] nbTriangles;
	public int[] quads = new int[0];
	static float SMALL_NUM = 0.00000001f;
	// 0 = disjoint (no intersect)
	// 1 = intersect in unique point I1
	// 2 = are in the same plane
	final static int DISJOINT = 0;
	final static int INTERSECT = 1;
	final static int SAME_PLANE = 2;

	public void calculateBounds(float[] xC, float[] yC, float[] zC,
			float[] bounds) {
		bounds[0] = Float.POSITIVE_INFINITY;
		bounds[1] = Float.POSITIVE_INFINITY;
		bounds[2] = Float.POSITIVE_INFINITY;
		bounds[3] = Float.NEGATIVE_INFINITY;
		bounds[4] = Float.NEGATIVE_INFINITY;
		bounds[5] = Float.NEGATIVE_INFINITY;
		for (int i = 0; i < xC.length; i++) {
			bounds[0] = Math.min(bounds[0], xC[i]);
			bounds[1] = Math.min(bounds[1], yC[i]);
			bounds[2] = Math.min(bounds[2], zC[i]);
			bounds[3] = Math.max(bounds[3], xC[i]);
			bounds[4] = Math.max(bounds[4], yC[i]);
			bounds[5] = Math.max(bounds[5], zC[i]);
		}
	}

	public void loadOBJ(String file) {
		DataInputStream in = null;
		BufferedReader br = null;
		try {
			FileInputStream fstream = new FileInputStream(file);
			in = new DataInputStream(fstream);
			br = new BufferedReader(new InputStreamReader(in));
			String strLine;
			try {
				int nV = 0;
				int nT = 0;
				int nQ = 0;
				// finding the length;
				strLine = br.readLine();
				while (strLine != null) {
					int comment = 0;
					if (strLine.startsWith("v ")) {
						nV++;
					} else if (strLine.startsWith("f ")) {

						String[] data = strLine.split(" ");
						if (data.length > 4 && isInteger(data[4])) {
							nQ++;

						} else {
							nT++;
						}
					}
					strLine = br.readLine();
				}
				// init arrays
				xc = new float[nV];
				yc = new float[nV];
				zc = new float[nV];
				triangles = new int[nT * 3];
				quads = new int[nQ * 4];
				System.out.println("Quads imported:" + nQ);
				System.out.println("Triangles imported:" + nT);
				// loading the data;
				fstream = new FileInputStream(file);
				in = new DataInputStream(fstream);
				br = new BufferedReader(new InputStreamReader(in));
				int iV = 0;
				int iT = 0;
				int iQ = 0;
				strLine = br.readLine();

				while (strLine != null) {
					if (strLine.startsWith("v ")) {
						String[] data = strLine.split(" ");
						xc[iV] = Float.valueOf(data[1]);
						yc[iV] = Float.valueOf(data[2]);
						zc[iV] = Float.valueOf(data[3]);
						iV++;
					} else if (strLine.startsWith("f ")) {
						String[] data = strLine.split(" ");
						if (data.length > 4 && isInteger(data[4])) {
							if (data.length > 5)
								System.out.println("too many vertices in face");
							for (int i = 1; i < 5; i++) {
								String[] number = data[i].split("/");

								quads[iQ] = Integer.valueOf(number[0]) - 1;
								iQ++;
							}
						} else {
							for (int i = 1; i < 4; i++) {
								String[] number = data[i].split("/");
								triangles[iT] = Integer.valueOf(number[0]) - 1;
								iT++;
							}
						}
					}
					strLine = br.readLine();
				}
			} catch (IOException e) {
				e.printStackTrace();
			}

		} catch (Exception e) {// Catch exception if any
			System.err.println("Error: " + e.getMessage());
		}
	}

	public void scaleToBox(float x1, float y1, float z1, float x2, float y2,
			float z2) {
		Box b3d = getBounds();
		float sX = (x2 - x1) / b3d.getDimX();
		float sY = (y2 - y1) / b3d.getDimY();
		float sZ = (z2 - z1) / b3d.getDimZ();
		this.scale(sX, sY, sZ);
		this.translate(x1 - b3d.x1 * sX, y1 - b3d.y1 * sY, z1 - b3d.z1 * sZ);
	}

	public void triangulateQuads() {
		int[] newtriangles = new int[triangles.length + (quads.length / 4) * 6];
		for (int i = 0; i < triangles.length; i++) {
			newtriangles[i] = triangles[i];
		}
		int triIndex = triangles.length;
		for (int i = 0; i < quads.length; i += 4) {
			newtriangles[triIndex] = quads[i];
			newtriangles[triIndex + 1] = quads[i + 1];
			newtriangles[triIndex + 2] = quads[i + 2];
			newtriangles[triIndex + 3] = quads[i + 2];
			newtriangles[triIndex + 4] = quads[i + 3];
			newtriangles[triIndex + 5] = quads[i];
			triIndex += 6;
		}
		triangles = newtriangles;
		quads = new int[0];
	}

	public static void saveObj(String file, float[] xc, float[] yc, float[] zc,
			int[] triangles, int[] quads) {
		FileWriter fw;
		try {
			fw = new FileWriter(file);
			BufferedWriter bw = new BufferedWriter(fw);
			for (int i = 0; i < xc.length; i++) {
				bw.write("v " + xc[i] + " " + yc[i] + " " + zc[i]);
				bw.newLine();
			}
			for (int i = 0; i < triangles.length; i += 3) {
				bw.write("f " + (triangles[i] + 1) + " "
						+ (triangles[i + 1] + 1) + " " + (triangles[i + 2] + 1));
				bw.newLine();
			}
			for (int i = 0; i < quads.length; i += 4) {
				bw.write("f " + (quads[i] + 1) + " " + (quads[i + 1] + 1) + " "
						+ (quads[i + 2] + 1) + " " + (quads[i + 3] + 1));
				bw.newLine();
			}
			bw.flush();
			bw.close();
			System.out.println("OBJ exported: " + file);
		} catch (IOException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
	}

	public void saveObj(String file) {
		saveObj(file, xc, yc, zc, triangles, quads);

	}

	public MMesh clipMesh(Box b3d) {
		int nTrisInside = 0, nQuadsInside = 0;
		int[] newC = new int[xc.length];
		Arrays.fill(newC, -1);
		int nVertices = 0;
		for (int i = 0; i < triangles.length; i += 3) {
			int nInside = 0;
			for (int j = 0; j < 3; j++) {
				int index = triangles[i + j];
				if (b3d.contains(xc[index], yc[index], zc[index])) {
					nInside++;
				}
			}
			if (nInside == 3) {
				nTrisInside++;
				for (int j = 0; j < 3; j++) {
					int index = triangles[i + j];
					if (newC[index] == -1) {
						newC[index] = nVertices;
						nVertices++;
					}
				}
			}
		}
		for (int i = 0; i < quads.length; i += 4) {
			int nInside = 0;
			for (int j = 0; j < 4; j++) {
				int index = quads[i + j];
				if (b3d.contains(xc[index], yc[index], zc[index])) {
					nInside++;
				}
			}
			if (nInside == 4) {
				nQuadsInside++;
				for (int j = 0; j < 4; j++) {
					int index = quads[i + j];
					if (newC[index] == -1) {
						newC[index] = nVertices;
						nVertices++;
					}
				}
			}
		}

		float[] newX = new float[nVertices];
		float[] newY = new float[nVertices];
		float[] newZ = new float[nVertices];

		for (int i = 0; i < newC.length; i++) {
			if (newC[i] >= 0) {
				newX[newC[i]] = xc[i];
				newY[newC[i]] = yc[i];
				newZ[newC[i]] = zc[i];
			}
		}
		int[] newTriangles = new int[nTrisInside * 3];
		int iTri = 0;
		for (int i = 0; i < triangles.length; i += 3) {
			int nInside = 0;
			for (int j = 0; j < 3; j++) {
				if (newC[triangles[i + j]] >= 0)
					nInside++;
			}
			if (nInside == 3) {
				for (int j = 0; j < 3; j++) {
					newTriangles[iTri] = newC[triangles[i + j]];
					iTri++;
				}
			}
		}
		int iQuads = 0;
		int[] newQuads = new int[nQuadsInside * 4];
		for (int i = 0; i < quads.length; i += 4) {
			int nInside = 0;
			for (int j = 0; j < 4; j++) {
				if (newC[quads[i + j]] >= 0)
					nInside++;
			}
			if (nInside == 4) {
				for (int j = 0; j < 4; j++) {
					newQuads[iQuads] = newC[quads[i + j]];
					iQuads++;
				}
			}
		}
		MMesh mesh = new MMesh();
		mesh.xc = newX;
		mesh.yc = newY;
		mesh.zc = newZ;
		mesh.triangles = newTriangles;
		mesh.quads = newQuads;
		return mesh;
	}

	public static boolean isInteger(String s) {
		try {
			Integer.parseInt(s);
		} catch (NumberFormatException e) {
			return false;
		}
		// only got here if we didn't return false
		return true;
	}

	public void createNormals() {
		int[] nFaces = new int[xc.length];
		xn = new float[xc.length];
		yn = new float[yc.length];
		zn = new float[zc.length];
		float[] result = new float[3];
		for (int i = 0; i < triangles.length; i += 3) {
			int i1 = triangles[i];
			int i2 = triangles[i + 1];
			int i3 = triangles[i + 2];
			normal(xc[i1], yc[i1], zc[i1], xc[i2], yc[i2], zc[i2], xc[i3],
					yc[i3], zc[i3], result);
			normalize(result, result);

			nFaces[i1]++;
			nFaces[i2]++;
			nFaces[i3]++;
			xn[i1] += result[0];
			yn[i1] += result[1];
			zn[i1] += result[2];

			xn[i2] += result[0];
			yn[i2] += result[1];
			zn[i2] += result[2];

			xn[i3] += result[0];
			yn[i3] += result[1];
			zn[i3] += result[2];
		}
		for (int i = 0; i < xn.length; i++) {
			xn[i] /= nFaces[i];
			yn[i] /= nFaces[i];
			zn[i] /= nFaces[i];
		}
	}

	public void createNbs() {
		for (int i = 0; i < triangles.length; i += 3) {
			int cId1 = 3;
			int cId2 = 0;
			for (int j = 0; j < 3; j++) {
				nodeNbs[j + cId1]++;
				nodeNbs[j + cId2]++;
				cId1 = cId2;
				cId2++;
			}
		}
	}

	public static void normal(float x1, float y1, float z1, float x2, float y2,
			float z2, float x3, float y3, float z3, float[] result) {
		float ux = x2 - x1;
		float uy = y2 - y1;
		float uz = z2 - z1;
		float vx = x3 - x1;
		float vy = y3 - y1;
		float vz = z3 - z1;
		float nx = uy * vz - uz * vy;
		float ny = uz * vx - ux * vz;
		float nz = ux * vy - uy * vx;
		float mag = (float) Math.sqrt(nx * nx + ny * ny + nz * nz);
		;
		nx /= mag;
		ny /= mag;
		nz /= mag;
		result[0] = nx;
		result[1] = ny;
		result[2] = nz;
	}

	public static void normalize(float[] normal, float[] result) {
		float nx = normal[0];
		float ny = normal[1];
		float nz = normal[2];
		float mag = (float) Math.sqrt(nx * nx + ny * ny + nz * nz);
		;
		nx /= mag;
		ny /= mag;
		nz /= mag;
		result[0] = nx;
		result[1] = ny;
		result[2] = nz;
	}

	public void scale(float sx, float sy, float sz) {
		for (int i = 0; i < xc.length; i++) {
			xc[i] *= sx;
			yc[i] *= sy;
			zc[i] *= sz;
		}
	}

	public void flipYZ() {
		for (int i = 0; i < xc.length; i++) {
			float temp = yc[i];
			yc[i] = zc[i];
			zc[i] = temp;
		}
	}

	public void translate(float sx, float sy, float sz) {
		for (int i = 0; i < xc.length; i++) {
			xc[i] += sx;
			yc[i] += sy;
			zc[i] += sz;
		}
	}

	

	public void display3D(PGraphics g3d) {
		g3d.beginShape(PGraphics.TRIANGLES);
		for (int i = 0; i < triangles.length; i++) {
			g3d.vertex(xc[triangles[i]], yc[triangles[i]], zc[triangles[i]]);
		}
		g3d.endShape();
		g3d.beginShape(PGraphics.QUADS);
		for (int i = 0; i < quads.length; i++) {
			g3d.vertex(xc[quads[i]], yc[quads[i]], zc[quads[i]]);
		}
		g3d.endShape();
	}

	public void display2D(PGraphics g3d) {
		g3d.beginShape(PGraphics.TRIANGLES);
		for (int i = 0; i < triangles.length; i++) {
			g3d.vertex(xc[triangles[i]], yc[triangles[i]]);
		}
		g3d.endShape();
		g3d.beginShape(PGraphics.QUADS);
		for (int i = 0; i < quads.length; i++) {
			g3d.vertex(xc[quads[i]], yc[quads[i]]);
		}
		g3d.endShape();
	}

	public static void write32(int a, byte[] buf, DataOutputStream dos)
			throws IOException {
		buf[3] = (byte) (a >>> 24);
		buf[2] = (byte) (a >> 16 & 0xff);
		buf[1] = (byte) (a >> 8 & 0xff);
		buf[0] = (byte) (a & 0xff);
		dos.write(buf, 0, 4);
	}

	public static void write32(float f, byte[] buf, DataOutputStream dos)
			throws IOException {
		write32(Float.floatToRawIntBits(f), buf, dos);
	}

	public void saveSTLBinary(String fileName) {
		saveSTLBinary(fileName, xc, yc, zc, triangles);
	}

	public static void saveSTLBinary(String fileName, float[] xc, float[] yc,
			float[] zc, int[] triangles) {
		System.out.println("saving STL...");
		try {
			FileOutputStream fos = new FileOutputStream(fileName);
			DataOutputStream dos = new DataOutputStream(
					new BufferedOutputStream(fos));
			// header
			dos.write(new byte[80], 0, 80);
			byte[] buf = new byte[4];
			int nTri = triangles.length / 3;
			write32(nTri, buf, dos);
			float[] normal = new float[3];
			for (int j = 0; j < triangles.length; j += 3) {
				int i1 = triangles[j];
				int i2 = triangles[j + 1];
				int i3 = triangles[j + 2];
				normal(xc[i1], yc[i1], zc[i1], xc[i2], yc[i2], zc[i2], xc[i3],
						yc[i3], zc[i3], normal);
				// normal
				write32(normal[0], buf, dos);
				write32(normal[1], buf, dos);
				write32(normal[2], buf, dos);
				// vertices
				write32(xc[i1], buf, dos);
				write32(yc[i1], buf, dos);
				write32(zc[i1], buf, dos);
				write32(xc[i2], buf, dos);
				write32(yc[i2], buf, dos);
				write32(zc[i2], buf, dos);
				write32(xc[i3], buf, dos);
				write32(yc[i3], buf, dos);
				write32(zc[i3], buf, dos);
				// info
				dos.writeByte(0 & 0xff);
				dos.writeByte(0 >> 8 & 0xff);
			}
			dos.flush();
			dos.close();
			fos.flush();
			fos.close();
		} catch (FileNotFoundException ex) {
			System.out.println("FileNotFoundException : " + ex);
		} catch (IOException ioe) {
		}
		System.out.println("STL saved.");
	}
	public boolean isInsideUpwards(float x, float y, float z) {
		TrianglePreCalc tri;
		float[] intersection = new float[3];
		boolean inside = false;
		for (int i = 0; i < triangles.length; i += 3) {
			int i1 = triangles[i];
			int i2 = triangles[i + 1];
			int i3 = triangles[i + 2];
			tri = new TrianglePreCalc(xc[i1], yc[i1], zc[i1], xc[i2], yc[i2],
					zc[i2], xc[i3], yc[i3], zc[i3]);
			int check = tri.triIntersect(x, y, z, 0, 0, 1, intersection);
			if (check == tri.INTERSECT) {
				return true;
			}
		}
		return false;
	}

	public boolean isInside(float x, float y, float z) {
		TrianglePreCalc tri;
		float[] intersection = new float[3];
		boolean inside = false;
		for (int i = 0; i < triangles.length; i += 3) {
			int i1 = triangles[i];
			int i2 = triangles[i + 1];
			int i3 = triangles[i + 2];
			tri = new TrianglePreCalc(xc[i1], yc[i1], zc[i1], xc[i2], yc[i2],
					zc[i2], xc[i3], yc[i3], zc[i3]);
			int check = tri.triIntersect(x, y, z, 0, 0, 1, intersection);
			if (check == tri.INTERSECT) {
				inside = !inside;
			}
		}
		return inside;
	}

	public void transform(PMatrix3D matrix) {
		float[] source = new float[3];
		float[] target = new float[3];
		for (int i = 0; i < xc.length; i++) {
			source[0] = xc[i];
			source[1] = yc[i];
			source[2] = zc[i];
			matrix.mult(source, target);
			xc[i] = target[0];
			yc[i] = target[1];
			zc[i] = target[2];
		}
	}

	public static void scanMeshDistanceAll(float[] xC, float[] yC, float[] zC,
			int[] faces, float x1, float y1, float z1, int nX, int nY, int nZ,
			float dim, float[] result) {
		for (int i = 0; i < result.length; i++) {
			result[i] = 100;
		}
		float cValue = 0;
		TrianglePreCalc tri;
		float[] triBounds = new float[6];
		for (int i = 0; i < faces.length; i += 3) {
			if (i % 100000 == 0) {
				System.out.println(i + "/" + faces.length);
			}
			int i1 = faces[i];
			int i2 = faces[i + 1];
			int i3 = faces[i + 2];
			tri = new TrianglePreCalc(xC[i1], yC[i1], zC[i1], xC[i2], yC[i2],
					zC[i2], xC[i3], yC[i3], zC[i3]);

			for (int x = 0; x < nX; x++) {
				for (int y = 0; y < nY; y++) {
					for (int z = 0; z < nZ; z++) {
						float cellX = x1 + x * dim;
						float cellY = y1 + y * dim;
						float cellZ = z1 + z * dim;
						float planeDist = (float) Math.abs(tri
								.getDistancePlane(cellX, cellY, cellZ));
						int index = x * nY * nZ + y * nZ + z;
						cValue = result[index];
						if (planeDist < cValue) {
							//
							float cDist = (float) tri.getDistance(cellX, cellY,
									cellZ);
							if (cDist < cValue) {
								result[index] = cDist;
							}
						}
					}
				}
			}
		}
	}

	public void scanMeshDistance(float x1, float y1, float z1, int nX, int nY,
			int nZ, float dim, float offset, float[] result) {
		scanMeshDistance(xc, yc, zc, triangles, x1, y1, z1, nX, nY, nZ, dim,
				offset, result);
	}

	public static void scanMeshDistanceContinue(float[] xC, float[] yC,
			float[] zC, int[] faces, float x1, float y1, float z1, int nX,
			int nY, int nZ, float dim, float offset, float[] result) {

		float cValue = 0;
		TrianglePreCalc tri;
		int nYZ = nY * nZ;
		float[] triBounds = new float[6];
		for (int i = 0; i < faces.length; i += 3) {
			if (i % 1000000 == 0) {
				System.out.println("scanned triangles: " + i + "/"
						+ faces.length);
			}

			int i1 = faces[i];
			int i2 = faces[i + 1];
			int i3 = faces[i + 2];
			tri = new TrianglePreCalc(xC[i1], yC[i1], zC[i1], xC[i2], yC[i2],
					zC[i2], xC[i3], yC[i3], zC[i3]);
			tri.calculateBounds(triBounds);
			triBounds[0] -= offset;
			triBounds[1] -= offset;
			triBounds[2] -= offset;
			triBounds[3] += offset;
			triBounds[4] += offset;
			triBounds[5] += offset;

			int cx1 = (int) ((triBounds[0] - x1) / dim) - 1;
			int cy1 = (int) ((triBounds[1] - y1) / dim) - 1;
			int cz1 = (int) ((triBounds[2] - z1) / dim) - 1;
			cx1 = Math.max(0, cx1);
			cy1 = Math.max(0, cy1);
			cz1 = Math.max(0, cz1);
			int cx2 = (int) ((triBounds[3] - x1) / dim) + 2;
			int cy2 = (int) ((triBounds[4] - y1) / dim) + 2;
			int cz2 = (int) ((triBounds[5] - z1) / dim) + 2;
			cx2 = Math.min(nX, cx2);
			cy2 = Math.min(nY, cy2);
			cz2 = Math.min(nZ, cz2);

			for (int x = cx1; x < cx2; x++) {
				for (int y = cy1; y < cy2; y++) {
					for (int z = cz1; z < cz2; z++) {
						float cellX = x1 + x * dim;
						float cellY = y1 + y * dim;
						float cellZ = z1 + z * dim;
						float planeDist = (float) Math.abs(tri
								.getDistancePlane(cellX, cellY, cellZ));

						int index = x * nY * nZ + y * nZ + z;

						cValue = result[index];
						 if (planeDist < cValue) {

						float cDist = (float) tri.getDistance(cellX, cellY,
								cellZ);
						result[index] = Math.min(cDist, cValue);

						 }
					}
				}
			}
		}
	}

	public static void scanMeshDistance(float[] xC, float[] yC, float[] zC,
			int[] faces, float x1, float y1, float z1, int nX, int nY, int nZ,
			float dim, float offset, float[] result) {
		int maxD = nX * nY * nZ;
		for (int i = 0; i < result.length; i++) {
			result[i] = maxD;
		}
		scanMeshDistanceContinue(xC, yC, zC, faces, x1, y1, z1, nX, nY, nZ,
				dim, offset, result);

	}

	public static void scanMeshBinary2(float[] xC, float[] yC, float[] zC,
			int[] faces, float x1, float y1, float z1, int nX, int nY, int nZ,
			float dim, float[] result) {
		float offset = dim;
		int maxD = nX * nY * nZ;
		int nYZ = nY * nZ;
		for (int i = 0; i < result.length; i++) {
			result[i] = 1;
		}
		float zStart = z1 - dim;
		float cValue = 0;
		TrianglePreCalc tri;
		float[] intersection = new float[3];
		float[] triBounds = new float[6];
		for (int i = 0; i < faces.length; i += 3) {
			if (i % 100000 == 0) {
				System.out.println("scanned triangles: " + i + "/"
						+ faces.length);
			}

			int i1 = faces[i];
			int i2 = faces[i + 1];
			int i3 = faces[i + 2];
			tri = new TrianglePreCalc(xC[i1], yC[i1], zC[i1], xC[i2], yC[i2],
					zC[i2], xC[i3], yC[i3], zC[i3]);
			tri.calculateBounds(triBounds);
			triBounds[0] -= offset;
			triBounds[1] -= offset;
			triBounds[2] -= offset;
			triBounds[3] += offset;
			triBounds[4] += offset;
			triBounds[5] += offset;

			int cx1 = (int) ((triBounds[0] - x1) / dim) - 1;
			int cy1 = (int) ((triBounds[1] - y1) / dim) - 1;

			cx1 = Math.max(0, cx1);
			cy1 = Math.max(0, cy1);

			int cx2 = (int) ((triBounds[3] - x1) / dim) + 2;
			int cy2 = (int) ((triBounds[4] - y1) / dim) + 2;

			cx2 = Math.min(nX, cx2);
			cy2 = Math.min(nY, cy2);

			for (int x = cx1; x < cx2; x++) {
				for (int y = cy1; y < cy2; y++) {
					float oX = x1 + x * dim;
					float oY = y1 + y * dim;
					int check = tri.triIntersect(oX, oY, zStart, 0, 0, 1,
							intersection);
					if (check == tri.INTERSECT) {
						int endZ = (int) ((intersection[2] - z1) / dim);
						for (int z = 0; z < endZ; z++) {
							int index = x * nYZ + y * nZ + z;
							if (result[index] == 0)
								result[index] = 1;

							else
								result[index] = 0;

						}
					}
				}
			}
		}

	}

	public static ArrayList<PVector> scanMeshBinary(float[] xC, float[] yC,
			float[] zC, int[] faces, float x1, float y1, float z1, int nX,
			int nY, int nZ, float dim, float[] result) {
		ArrayList<PVector> intersectors = new ArrayList<PVector>();
		float[] intersection = new float[3];
		ArrayList<Float> intersections = new ArrayList<Float>();
		TrianglePreCalc triangle = new TrianglePreCalc();
		for (int x = 0; x < nX; x++) {
			// System.out.println("process: "+x+"/"+nX);
			for (int y = 0; y < nY; y++) {
				intersections.clear();
				// ray in z-direction
				float oX = x * dim + x1;
				float oY = y * dim + y1;
				float oZ = z1;
				// collect all intersections
				for (int i = 0; i < faces.length; i += 3) {
					int i1 = faces[i];
					int i2 = faces[i + 1];
					int i3 = faces[i + 2];
					int check = triangle.triIntersect(oX, oY, oZ, 0, 0, 1,
							xC[i1], yC[i1], zC[i1], xC[i2], yC[i2], zC[i2],
							xC[i3], yC[i3], zC[i3], intersection);
					if (check == INTERSECT) {
						intersections.add(intersection[2]);
						intersectors.add(new PVector(oX, oY,
								(float) intersection[2]));
					}
				}
				Collections.sort(intersections);
				int startI = 1;
				if (intersections.size() % 2 == 0)
					startI = 0;
				for (int i = startI; i < intersections.size() - 1; i += 2) {
					int zStart = (int) ((intersections.get(i) - z1) / dim);
					int zEnd = (int) ((intersections.get(i + 1) - z1) / dim);
					for (int cZ = zStart; cZ < zEnd; cZ++) {
						int index = x * nY * nZ + y * nZ + cZ;
						result[index] = 1;
					}

				}
			}
		}
		return intersectors;
	}

	public static float dot(float x1, float y1, float z1, float x2, float y2,
			float z2) {
		return x1 * x2 + y1 * y2 + z1 * z2;
	}

	public MMesh clone() {
		MMesh copyMesh = new MMesh();
		copyMesh.xc = Arrays.copyOf(xc, xc.length);
		copyMesh.yc = Arrays.copyOf(yc, yc.length);
		copyMesh.zc = Arrays.copyOf(zc, zc.length);
		copyMesh.triangles = Arrays.copyOf(triangles, triangles.length);
		copyMesh.quads = Arrays.copyOf(quads, quads.length);
		return copyMesh;
	}

	@Override
	public void setPosition(int i, float x, float y, float z) {
		// TODO Auto-generated method stub
		xc[i] = x;
		yc[i] = y;
		zc[i] = z;
	}

	

	@Override
	public int getNumVertices() {
		// TODO Auto-generated method stub
		return xc.length;
	}

	@Override
	public void getPosition(int i, float[] coords) {
		// TODO Auto-generated method stub
		coords[0] = xc[i];
		coords[1] = yc[i];
		coords[2] = zc[i];
	}
	public PVector getIntersection(int i1,int i2,Plane plane){
		float vx=xc[i1];
		float vy=yc[i1];
		float vz=zc[i1];
		float dx=xc[i2]-vx;
		float dy=yc[i2]-vy;
		float dz=zc[i2]-vz;
		return plane.linePlaneIntersection(vx, vy, vz, dx, dy, dz);
	}
	public PVector getPoint(int i){
		return new PVector(xc[i],yc[i],zc[i]);
	}
	public void removeUnusedNodes(){
		int[]newIndex=new int[xc.length];
		
	}
	public int[] clip(float x,float y,float z,float nx,float ny,float nz){
		Plane plane=new Plane(x,y,z,nx,ny,nz);
		return clipMesh(plane);
	}
	public int[] clipMesh(Plane plane){
		//must be triangulated
		boolean[]inside=new boolean[xc.length];
		int[]newIndex=new int[xc.length];
		int nIn=0;
		for (int i=0;i<this.xc.length;i++){
			float x=xc[i];
			float y=yc[i];
			float z=zc[i];
			if (plane.getDistance(x, y, z)<=0){
				inside[i]=true;
				nIn++;
			}
		}
		int nNewTriangles=0;
		int nNewPoints=0;
		int nEdges=0;
		for (int i = 0; i < this.triangles.length; i += 3) {
			int nInside=0;
			for (int j = 0; j <  3; j++) {
				if (inside[triangles[i+j]]){
					nInside++;
				}
			}
			if (nInside==1){
				nNewPoints+=2;
				nNewTriangles++;
				nEdges+=2;
			}
			else if (nInside==2){
				nNewPoints+=2;
				nNewTriangles+=2;
				nEdges+=2;
			}
			else if (nInside==3){
				nNewTriangles++;
			}
		}
		float[]xcNew=new float[nNewPoints+nIn];
		float[]ycNew=new float[nNewPoints+nIn];
		float[]zcNew=new float[nNewPoints+nIn];
		int[]edges=new int[nEdges];
		nIn=0;
		for (int i=0;i<this.xc.length;i++){
			if (inside[i]){
				newIndex[i]=nIn;
				xcNew[nIn]=xc[i];
				ycNew[nIn]=yc[i];
				zcNew[nIn]=zc[i];
				nIn++;
			}
		}
		int[]triNew=new int[nNewTriangles*3];
		int[]insides=new int[3];
		int cTri=0;
		int cEdge=0;
		for (int i = 0; i < this.triangles.length; i += 3) {
			int nInside=0;
			for (int j = 0; j <  3; j++) {
				if (inside[triangles[i+j]]){
					insides[nInside]=j;
					nInside++;
				}
			}
			if (nInside==1){
				int iIn=insides[0];
				int iOut1=(iIn+1)%3;
				int iOut2=(iIn+2)%3;
				iIn=triangles[i+iIn];
				iOut1=triangles[i+iOut1];
				iOut2=triangles[i+iOut2];
				PVector inter1=getIntersection(iIn,iOut1,plane);
				PVector inter2=getIntersection(iIn,iOut2,plane);
				
				
				xcNew[nIn]=inter1.x;
				ycNew[nIn]=inter1.y;
				zcNew[nIn]=inter1.z;
				edges[cEdge]=nIn;
				cEdge++;
				nIn++;
				xcNew[nIn]=inter2.x;
				ycNew[nIn]=inter2.y;
				zcNew[nIn]=inter2.z;
				edges[cEdge]=nIn;
				cEdge++;
				nIn++;
				
				triNew[cTri]=newIndex[iIn];
				cTri++;
				triNew[cTri]=nIn-2;
				cTri++;
				triNew[cTri]=nIn-1;
				cTri++;
				
				
			}
			if (nInside==2){
				int iIn1=insides[0];
				int iIn2=insides[1];
				if (insides[1]-insides[0]==2){
					iIn2=insides[0];
					iIn1=insides[1];
				}
				int iOut=(iIn2+1)%3;
				iIn1=triangles[i+iIn1];
				iIn2=triangles[i+iIn2];
				iOut=triangles[i+iOut];
				
				PVector inter1=getIntersection(iIn1,iOut,plane);
				PVector inter2=getIntersection(iIn2,iOut,plane);
				
				
				xcNew[nIn]=inter1.x;
				ycNew[nIn]=inter1.y;
				zcNew[nIn]=inter1.z;
				edges[cEdge]=nIn;
				cEdge++;
				nIn++;
				xcNew[nIn]=inter2.x;
				ycNew[nIn]=inter2.y;
				zcNew[nIn]=inter2.z;
				edges[cEdge]=nIn;
				cEdge++;
				nIn++;
				
				triNew[cTri]=newIndex[iIn1];
				cTri++;
				triNew[cTri]=nIn-1;
				cTri++;
				triNew[cTri]=nIn-2;
				cTri++;
				
				triNew[cTri]=newIndex[iIn1];
				cTri++;
				triNew[cTri]=newIndex[iIn2];
				cTri++;
				triNew[cTri]=nIn-1;
				cTri++;
			}
			if (nInside==3){
				triNew[cTri]=newIndex[triangles[i]];
				cTri++;
				triNew[cTri]=newIndex[triangles[i+1]];
				cTri++;
				triNew[cTri]=newIndex[triangles[i+2]];
				cTri++;
			}
		}
		triangles=triNew;
		xc=xcNew;
		yc=ycNew;
		zc=zcNew;
		return edges;
	}
}
	

